/*
 * Copyright (c) 2024 Black Sesame Technologies
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <mutex>
#include <random>
#include <cstdint>
#include <thread>
#include <chrono>
#include <fastdds/dds/domain/DomainParticipant.hpp>
#include <fastdds/dds/publisher/Publisher.hpp>
#include <fastdds/dds/subscriber/Subscriber.hpp>
#include <fastdds/dds/subscriber/DataReader.hpp>
#include "dal_dmabufheap/DmaBufHeapManager.hpp"
#include "dal_dmabufheap/DbhNotifyMsgDef.hpp"
#include "dal_fastdds/dbh/DalDbhReliableWriter.hpp"
#include "dal_fastdds/utils/Uuid.hpp"
#include "dal_fastdds/utils/log.hpp"
#include "dal_utils/MD5Helper.hpp"

DalDbhReliableWriter::DalDbhReliableWriter() {}

DalDbhReliableWriter::~DalDbhReliableWriter()
{
    if (!SendCloseFdsDbhInfo()) {
        LOG_ERROR_WITH_LOCATION() << "SendCloseFdsDbhInfo failed" << std::endl;
    }

    if (writer_dbh_listener_)
        delete writer_dbh_listener_;
    if (reader_dbh_listener_)
        delete reader_dbh_listener_;
    if (ringbuffer_)
        delete ringbuffer_;
}

bool DalDbhReliableWriter::CreateDbhSendNotifyWriter()
{
    DataWriterQos f_qos;
    f_qos.reliability().kind = RELIABLE_RELIABILITY_QOS;
    f_qos.history().kind = KEEP_ALL_HISTORY_QOS;
    f_qos.history().depth = 5;
    f_qos.durability().kind = VOLATILE_DURABILITY_QOS;

#ifdef DAL_FASTDDS_DATA_SHARING_DISABLED
    f_qos.data_sharing().off();
#endif

    if (dbh_send_notify_writer_listener_) {
        writer_dbh_listener_ = new DataWriterListenerImplDbhExtend(
            dbh_send_notify_writer_listener_->GetCallbacks(), dbh_send_notify_writer_status_mask_);
    } else {
        dal_datawriter_listener_callbacks_t callbacks_;
        memset(&callbacks_, 0, sizeof(dal_datawriter_listener_callbacks_t));
        writer_dbh_listener_ =
            new DataWriterListenerImplDbhExtend(callbacks_, dbh_send_notify_writer_status_mask_);
    }

    // !TODO listener
    dbh_send_notify_writer_ = fastdds_pub_->create_datawriter(
        dbh_send_notify_topic_, f_qos, writer_dbh_listener_, dbh_send_notify_writer_status_mask_);
    if (dbh_send_notify_writer_) {
        return true;
    }
    return false;
}

bool DalDbhReliableWriter::CreateDbhRecvNotifyReader()
{
    dbh_recv_notify_subscriber_ = fastdds_participant_->create_subscriber(SUBSCRIBER_QOS_DEFAULT);

    DataReaderQos f_qos;
    f_qos.reliability().kind = RELIABLE_RELIABILITY_QOS;
    f_qos.history().kind = KEEP_ALL_HISTORY_QOS;
    f_qos.history().depth = 5;
    f_qos.durability().kind = VOLATILE_DURABILITY_QOS;

#ifdef DAL_FASTDDS_DATA_SHARING_DISABLED
    f_qos.data_sharing().off();
#endif

    if (!ringbuffer_) {
        ringbuffer_ = new BufferDescriptorRingBuffer(RINGBUFFER_LEN);
    }
    reader_dbh_listener_ =
        new DataReaderListenerImplDbhExtend(dbh_recv_notify_msg_ts_callbacks_, ringbuffer_);

    dbh_recv_notify_reader_ = dbh_recv_notify_subscriber_->create_datareader(
        dbh_recv_notify_topic_, f_qos, reader_dbh_listener_, StatusMask::data_available());
    if (dbh_recv_notify_reader_)
        return true;
    return false;
}

bool DalDbhReliableWriter::Write(void* msg)
{
    if (!InitRingBuffer(msg)) {
        LOG_ERROR_WITH_LOCATION() << "init ring buffer failed." << std::endl;
    }

    if (!CheckResize(msg)) {
        LOG_ERROR_WITH_LOCATION() << "check resize failed." << std::endl;
    }

    {
        std::lock_guard<std::mutex> lock(ringbuffer_->ringbuffer_mutex_);

        // serialize msg to block with extra copy.
        auto& buffer_desc_ = ringbuffer_->buffer_[ringbuffer_->GetWriteIndex()];
        auto& block_tmp_ = ringbuffer_->buffer_[ringbuffer_->GetWriteIndex()].dbh_block_;

        eprosima::fastrtps::rtps::SerializedPayload_t payload;
        payload.max_size = block_tmp_.size;
        payload.data = reinterpret_cast<uint8_t*>(block_tmp_.ptr);
        eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast<char*>(payload.data),
                                                 payload.max_size);
        eprosima::fastcdr::Cdr ser(
            fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
        if (msg_ts_->serializeROSmessage(msg, ser, msg_ts_callbacks_)) {
            payload.encapsulation =
                ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE;
            payload.length = (uint32_t)ser.getSerializedDataLength();
        } else {
            LOG_ERROR_WITH_LOCATION() << "serializeROSmessage failed." << std::endl;
            return false;
        }

        if (is_debug_print_enabled_) {
            std::cout << "[dbh send notify writer ------>] md5sum = ";
            dal_utils::PrintMD5(payload.data, estimated_size);
        }

        // sync
        DmaBufHeapManager* h = DmaBufHeapManager::GetInstance();
        h->SyncBlockWriteMode(&block_tmp_);

        // avoid segment fault when SerializedPayload_t destructor is called.
        payload.data = nullptr;

        uint8_t uuid[UUID_LEN];
        GenUUID(uuid);

        // set BufferDescriptor
        buffer_desc_.ref_cnt_.store(
            writer_dbh_listener_->matched_readers_cnt_map_.Get(dbh_send_notify_writer_->guid()));
        buffer_desc_.ref_cnt_initial_ = buffer_desc_.ref_cnt_.load();
        memcpy(buffer_desc_.uuid_, uuid, UUID_LEN);

        // set send notify msg.
        dbh_send_notify_msg_.msg_size = estimated_size;
        dbh_send_notify_msg_.global_fd = block_tmp_.global_fd;
        dbh_send_notify_msg_.mmap_size = block_tmp_.size;
        memcpy(dbh_send_notify_msg_.uuid.data(), uuid, UUID_LEN);

        // send send notify msg.
        InstanceHandle_t f_handle;
        dal_fastrtps_shared_cpp::SerializedData sdata;
        sdata.is_cdr_buffer = false;
        sdata.data = static_cast<void*>(&dbh_send_notify_msg_);
        sdata.impl = dbh_send_notify_msg_ts_callbacks_;

        if (DAL_RETCODE_OK != dbh_send_notify_writer_->write(&sdata, f_handle)) {
            std::cout << "dbh send notify writer write failed. " << std::endl;
            return false;
        }
        // for debug
        if (is_debug_print_enabled_) {
            std::cout << "[dbh send notify writer ------>]" << std::dec
                      << "  topic = " << dbh_send_notify_topic_name_
                      << ", initial matched reader cnt = " << buffer_desc_.ref_cnt_.load()
                      << ", global_fd = " << dbh_send_notify_msg_.global_fd
                      << ", mmap_size = " << dbh_send_notify_msg_.mmap_size
                      << ", msg_size = " << dbh_send_notify_msg_.msg_size
                      << ", index = " << dbh_send_notify_msg_.index << ", UUID = ";
            for (const auto& byte : dbh_send_notify_msg_.uuid) {
                std::cout << std::hex << +byte << " ";  // 将每个字节转换为十六进制表示并打印
            }
            std::cout << std::dec << std::endl;
        }
    }

    uint32_t cnt = WRITE_TIMEOUT;
    while (cnt--) {
        auto& buffer_desc_ = ringbuffer_->buffer_[ringbuffer_->GetWriteIndex()];

        // Get matched_reader cnt now and compare initial matched reader cnt.
        uint32_t cur_matched_reader_cnt =
            writer_dbh_listener_->matched_readers_cnt_map_.Get(dbh_send_notify_writer_->guid());
        uint32_t expected_ref_cnt = buffer_desc_.ref_cnt_initial_ - cur_matched_reader_cnt;

        {
            std::lock_guard<std::mutex> lock(ringbuffer_->ringbuffer_mutex_);
            if (buffer_desc_.ref_cnt_.load() <= expected_ref_cnt) {
                break;
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
    // whatever recved enough recv_notify_msg from listeners, writer_p always add
    ringbuffer_->writer_p_++;

    return true;
}

void DalDbhReliableWriter::GenUUID(uint8_t* ptr)
{
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<uint8_t> dis(0, 255);

    for (int i = 0; i < UUID_LEN; ++i) {
        ptr[i] = dis(gen);
    }
}